#include "mpu9250.h"
#include "stdio.h"
#include "math.h"

MPU9250::MPU9250(I2C_HandleTypeDef *_p_i2c)
{
	p_i2c = _p_i2c;
	/*
	 single_write(GYRO_ADDRESS,PWR_M, 0x80);   //
	 single_write(GYRO_ADDRESS,SMPL, 0x07);    //
	 single_write(GYRO_ADDRESS,DLPF, 0x1E);    //±2000°
	 single_write(GYRO_ADDRESS,INT_C, 0x00 );  //
	 single_write(GYRO_ADDRESS,PWR_M, 0x00);   //
	 */
	//电源唤醒
	single_write(GYRO_ADDRESS, PWR_MGMT_1, 0x00);	//解除休眠状态
	single_write(GYRO_ADDRESS, SMPLRT_DIV, 0x07);
	single_write(GYRO_ADDRESS, CONFIG, 0x06);
	//角速度 量程设置,0x18(不自检，2000deg/s)
	single_write(GYRO_ADDRESS, GYRO_CONFIG, 0x18);
	//加速度 量程设置,0x01(不自检，2G，5Hz)
	single_write(GYRO_ADDRESS, ACCEL_CONFIG, 0x01);

	// ---- 读磁力计

	//0x37 ==55,写的寄存器55，打开bypass
	single_write(GYRO_ADDRESS, MPU9250_RA_INT_PIN_CFG, 0x02);  //turn on Bypass Mode
	HAL_Delay(100);
	uint8_t who_am_i = single_read(MAG_ADDRESS, 0x00);
	if (who_am_i != 0x48)
	{
		// 错误处理
	}
	//CNTL, mode0 =1, 单次测量
	single_write(MAG_ADDRESS, 0x0A, 0x1F);
	HAL_Delay(10);
	//----------------
//	single_write(GYRO_ADDRESS,0x6A,0x00);//close Master Mode

}

void MPU9250::Scan_I2C_Addresses() {
    uint8_t address;
    HAL_StatusTypeDef status;

    for (address = 0x08; address <= 0x77; address++) {
        // 尝试启动一个读操作
        status = HAL_I2C_IsDeviceReady(p_i2c, (address << 1), 2, 10);  // 注意：地址需要左移一位

        if (status == HAL_OK) {
            // 如果收到 ACK，则表示该地址有设备
            printf("Device found at address: 0x%02X\n", address);
        }
    }
}

//******读取MPU9250数据****************************************
void MPU9250::read_accel(int16_t *p_data_acc)
{
	//最大量程2g

	BUF[0] = single_read(ACCEL_ADDRESS, ACCEL_XOUT_L);
	BUF[1] = single_read(ACCEL_ADDRESS, ACCEL_XOUT_H);
	T_X = (BUF[1] << 8) | BUF[0];
	// 16bit = [-32768 , +32767], 16384 lsb /g
	T_X /= 164; 						   //读取计算X轴数据
	p_data_acc[0] = T_X;

	BUF[2] = single_read(ACCEL_ADDRESS, ACCEL_YOUT_L);
	BUF[3] = single_read(ACCEL_ADDRESS, ACCEL_YOUT_H);
	T_Y = (BUF[3] << 8) | BUF[2];
	T_Y /= 164; 						   //读取计算Y轴数据
	p_data_acc[1] = T_Y;

	BUF[4] = single_read(ACCEL_ADDRESS, ACCEL_ZOUT_L);
	BUF[5] = single_read(ACCEL_ADDRESS, ACCEL_ZOUT_H);
	T_Z = (BUF[5] << 8) | BUF[4];
	T_Z /= 164; 					       //读取计算Z轴数据
	p_data_acc[2] = T_Z;


}

void MPU9250::read_gyro(int16_t *p_data)
{

	BUF[0] = single_read(GYRO_ADDRESS, GYRO_XOUT_L);
	BUF[1] = single_read(GYRO_ADDRESS, GYRO_XOUT_H);
	T_X = (BUF[1] << 8) | BUF[0];
	T_X /= 16.4; 						   //读取计算X轴数据
	p_data[0] = T_X;

	BUF[2] = single_read(GYRO_ADDRESS, GYRO_YOUT_L);
	BUF[3] = single_read(GYRO_ADDRESS, GYRO_YOUT_H);
	T_Y = (BUF[3] << 8) | BUF[2];
	T_Y /= 16.4; 						   //读取计算Y轴数据
	p_data[1] = T_Y;

	BUF[4] = single_read(GYRO_ADDRESS, GYRO_ZOUT_L);
	BUF[5] = single_read(GYRO_ADDRESS, GYRO_ZOUT_H);
	T_Z = (BUF[5] << 8) | BUF[4];
	T_Z /= 16.4; 					       //读取计算Z轴数据
	p_data[2] = T_Z;

	// BUF[6]=single_read(GYRO_ADDRESS,TEMP_OUT_L);
	// BUF[7]=single_read(GYRO_ADDRESS,TEMP_OUT_H);
	// T_T=(BUF[7]<<8)|BUF[6];
	// T_T = 35+ ((double) (T_T + 13200)) / 280;// 读取计算出温度
}

void MPU9250::read_mag(int16_t *p_data)
{


	//读取计算X轴数据
	BUF[0] = single_read(MAG_ADDRESS, MAG_XOUT_L);
	BUF[1] = single_read(MAG_ADDRESS, MAG_XOUT_H);
	T_X = (BUF[1] << 8) | BUF[0];
	p_data[0] = T_X;

	//读取计算Y轴数据
	BUF[2] = single_read(MAG_ADDRESS, MAG_YOUT_L);
	BUF[3] = single_read(MAG_ADDRESS, MAG_YOUT_H);
	T_Y = (BUF[3] << 8) | BUF[2];
	p_data[1] = T_Y;

	//读取计算Z轴数据
	BUF[4] = single_read(MAG_ADDRESS, MAG_ZOUT_L);
	BUF[5] = single_read(MAG_ADDRESS, MAG_ZOUT_H);
	T_Z = (BUF[5] << 8) | BUF[4];
	p_data[2] = T_Z;
}

bool MPU9250::single_write(uint8_t SlaveAddress, uint8_t REG_Address,
		uint8_t REG_data)		     //void
{
	HAL_StatusTypeDef result = HAL_I2C_Mem_Write(p_i2c, SlaveAddress,
			REG_Address, 1, &REG_data, 1, 1000);
	bool is_success = false;
	if (result == HAL_OK)
	{
		is_success = true;
	}
	else
	{
		is_success = false;
	}
	HAL_Delay(5);

	return is_success;

}

//单字节读取*****************************************
uint8_t MPU9250::single_read(uint8_t SlaveAddress, uint8_t REG_Address)
{
	uint8_t result = 0;

	if (HAL_I2C_Mem_Read(p_i2c, SlaveAddress | 0x01, REG_Address, 1, &result, 1, 1000)
			== HAL_OK)
	{
		return result;
	}
	else
	{
		return -1;
	}


}

